#include "encoder.h"
#include "ti_msp_dl_config.h"

volatile int16_t right_speed_run = 0;
volatile int16_t left_speed_run = 0;
volatile int16_t left_speed_last = 0;
volatile int16_t right_speed_last = 0;
volatile int32_t Left_Count = 0;
volatile int32_t Right_Count = 0;

void Encoder_Init()
{
    NVIC_EnableIRQ(GPIO_Encoder_INT_IRQN);
}

void GROUP1_IRQHandler(void)
{
    uint32_t gpioA_R = DL_GPIO_getEnabledInterruptStatus(GPIOA, GPIO_Encoder_PIN_Right_A_PIN );
    uint32_t gpioB_R = DL_GPIO_getEnabledInterruptStatus(GPIOA, GPIO_Encoder_PIN_Right_B_PIN );

    uint32_t gpioA_L = DL_GPIO_getEnabledInterruptStatus(GPIOA, GPIO_Encoder_PIN_Left_A_PIN );
    uint32_t gpioB_L = DL_GPIO_getEnabledInterruptStatus(GPIOA, GPIO_Encoder_PIN_Left_B_PIN );

    if (gpioA_R & GPIO_Encoder_PIN_Right_A_PIN) 
    {
        if(DL_GPIO_readPins(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Right_B_PIN))
            Right_Count--;
        else 
            Right_Count++;
        DL_GPIO_clearInterruptStatus(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Right_A_PIN);
    }

    else if(gpioB_R & GPIO_Encoder_PIN_Right_B_PIN)
    {
        if(DL_GPIO_readPins(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Right_A_PIN))
            Right_Count++;
        else 
            Right_Count--;
        DL_GPIO_clearInterruptStatus(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Right_B_PIN);
    }

    if (gpioA_L & GPIO_Encoder_PIN_Left_A_PIN) 
    {
        if(DL_GPIO_readPins(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Left_B_PIN))
            Left_Count--;
        else 
            Left_Count++;
        DL_GPIO_clearInterruptStatus(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Left_A_PIN);
    }

    else if(gpioB_L & GPIO_Encoder_PIN_Left_B_PIN)
    {
        if(DL_GPIO_readPins(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Left_A_PIN))
            Left_Count++;
        else 
            Left_Count--;
        DL_GPIO_clearInterruptStatus(GPIO_Encoder_PORT, GPIO_Encoder_PIN_Left_B_PIN);
    }
}


void Get_speed()//编码器获取速度
{
    right_speed_run = Right_Count / 4.80 * 15.08;        //将速度转化为cm/s,6.60为直径，21.37为周长
    left_speed_run  = Left_Count / 4.80 * 15.08;

    left_speed_run  = 0.7 * left_speed_last + 0.3 * left_speed_run;            //一阶低通滤波
    right_speed_run = 0.7 * right_speed_last + 0.3 * right_speed_run;

    left_speed_last  = left_speed_run;
    right_speed_last = right_speed_run;

    Right_Count = 0;
    Left_Count  = 0;
}


int32_t Encoder_Get(uint8_t Choose)
{
    int32_t temp;
    if (Choose == 1)
    {
        temp = Left_Count;
        Left_Count = 0; // 读取后清零
    }
    else if (Choose == 2)
    {
        temp = Right_Count;
        Right_Count = 0; // 读取后清零
    }

    return temp;
}